#include <stdio.h>
#include <stdlib.h>
#include "robot.h"

/* Testea la funcion SOLVE del capítulo 3 */

int main(void)
{
    vec3 v_trels_values[4] = {{0.0, 0.0, -90.0}, {0.6, -0.3, 45.0},\
                              {-0.4, 0.3, 120.0}, {0.8, 0.4, 30.0}};
    vec3 v_trelw = {0.1, 0.2, 30.0};
    vec3 v_srelb = {-0.1, 0.3, 0.0};
    vec3 current = {0.0, 0.0, 0.0};
    vec3 uform, near, far;
    frame *trelw = UTOI(v_trelw);
    frame *srelb = UTOI(v_srelb);
    Bool sol = FALSE;
    frame *trels;
    frame *aux;
    int i=0, j=0;
    
    for (i=0; i<4; i++)
    {
        trels = UTOI(v_trels_values[i]);
        sol = SOLVE(trels, trelw, srelb, current, near, far);
        if (!sol)
        {
            printf("No hay solucion\n");
            gsl_matrix_free(trels);
            break;
        }
        else
        {
            for (j=0; j<3; j++)
                current[j] = near[j];
	    printf("angulos de articulacion solucion near: ");
            print_vec3(near, stdout);
	    printf("angulos de articulacion solucion far: ");
            print_vec3(far, stdout);
            printf("Verificación:\n");
            aux = WHERE(near, trelw, srelb);
            ITOU(aux, uform);
	    printf("trels en near: ");
            print_vec3(uform, stdout);
            gsl_matrix_free(aux);
            aux = WHERE(far, trelw, srelb);
            ITOU(aux,uform);
	    printf("trels en far: ");
            print_vec3(uform, stdout);
            gsl_matrix_free(aux);
        }
        gsl_matrix_free(trels);
    }
    gsl_matrix_free(trelw);
    gsl_matrix_free(srelb);
    return 0;
}
